#include "headfile.h"

int16 limit_int16(int16* p,int16 n)
{
	int16 temp=*p;
	if(temp>n)return n;
	else if(temp<-n)return(-n);
	else return temp;
}
float limit_float(float* p,float n)
{
	float temp=*p;
	if(temp>n)return n;
	else if(temp<-n) return(-n);
	else return temp;
}
float fuzzy_pidmode(int16 gyro_x)
{
	float x;
	if(gyro_x<0)gyro_x=-gyro_x;
	if(gyro_x>3000) return 0;
	else if(gyro_x<1000)return 1;
	else 
	{
		x=((float)gyro_x-1000)/2000;
		return x;
	}
}
void fill(float *s,float a,float b,float c,float d,float e)
{
	s[0]=a;
	s[1]=b;
	s[2]=c;
	s[3]=d;
	s[4]=e;
}
//void fuzzy_scale5(float p[5],float x,float scale1,float scale2)
//{
//	float a,b;
//	if(x<0-scale2)      fill(p,1,0,0,0,0);
//	else if(x<-scale1)
//	{
//		a=1/(scale1-scale2)*(x-scale1);
//		b=1-a;
//		fill(p,a,b,0,0,0);
//	}
//	else if(x<0)
//	{
//		a=-x/scale1;
//		b=1-a;
//		fill(p,0,a,b,0,0);
//	}
//	else if(x<scale1)
//	{
//		b=x/scale1;
//		a=1-b;
//		fill(p,0,0,a,b,0);
//	}
//	else if(x<scale2)
//	{
//		b=1/(scale2-scale1)*(x-scale1);
//		a=1-b;
//		fill(p,0,0,0,a,b);
//	}
//	else if(x>scale2)    fill(p,0,0,0,0,1);
//}


int fuzzy_scale3(float x,float scale)
{
	if(x<-scale)return(0);
	else if(x<scale)return(1);
	else return(2);
}